package regulators;

import gui.OpCom;
import gui.PlotData;
import monitor.ControllerStatMonitor;
import monitor.ExcecutionMonitor;
import monitor.StateVariableMonitor;
import se.lth.control.DoublePoint;
import se.lth.control.realtime.*;
import controllers.PI;
import controllers.PID;
import controllers.PIDParameters;
import controllers.PIParameters;
import SimEnvironment.*;

public class BeamRegul extends Thread implements Regul{
	
	
	public ExcecutionMonitor excecutionMonitor = new ExcecutionMonitor();
	StateVariableMonitor stateVariableMonitor;
	public ControllerStatMonitor controllerStatMonitor;
	private PID controller;
	private double ref;
	private AnalogIn analogInAngle;
	private AnalogOut analogOut;
	private double uMin = -10.0;
	private double uMax = 10.0;
	private double y;
	private double u;
	public PIDParameters innerContParameters;
	public BeamRegul(AnalogIn analogInPos, AnalogIn analogInAngle, AnalogOut analogOut, double ref, int pri, OpCom op, StateVariableMonitor stateVariableMonitor, ControllerStatMonitor controllerStatMonitor) {
		this.ref = ref;
		this.analogInAngle = analogInAngle;
		this.analogOut = analogOut;
		controller = new PID("PID");
		
		innerContParameters = new PIDParameters();
		innerContParameters.K = 5.0;
		innerContParameters.Ti = 5.0;
		innerContParameters.Tr = 7.0;
		innerContParameters.Beta = 1;
		innerContParameters.H = 0.01;
		innerContParameters.Td = 0.25;
		innerContParameters.N = 5.0;
		innerContParameters.integratorOn = true;
		setInnerParameters(innerContParameters);
		this.stateVariableMonitor = stateVariableMonitor;
		this.controllerStatMonitor = controllerStatMonitor;
		excecutionMonitor = new ExcecutionMonitor();
		excecutionMonitor.setRunning(false);
		setPriority(pri);
	}
	
	
	private double limit(double u, double umin, double umax) {
		if (u < umin) {
			u = umin;
		} else if (u > umax) {
			u = umax;
		} 
		return u;
	}
	
	@Override
	public void run() {
		long t;
		 y = 0;
	while(true){
		
			t =  System.currentTimeMillis();
			
			try {
				y = analogInAngle.get();
				controllerStatMonitor.setBeamAngle1(y);
				
			} catch (IOChannelException e1) {
				
				e1.printStackTrace();
			}
			
			
			synchronized (controller) { 
				
				 u = limit(controller.calculateOutput(y, ref), uMin, uMax);
				
				if(isRunning()) {
				try {
					
					analogOut.set(u);
					controllerStatMonitor.setControlSignal(u);
				} catch (IOChannelException e) {
					
					e.printStackTrace();
				}
				// Update state
				
			}
			
			controller.updateState(u);
			t = t + controller.getHMillis();
			long duration = t - System.currentTimeMillis();
			if (duration > 0) {
				try {
					sleep(duration);
				} catch (InterruptedException e) {
					e.printStackTrace();
				}
			
			}
		}
	}
	}
	
	
	public boolean isRunning() {
		
		return excecutionMonitor.isRunning();
	}
	
	public void restart() {
		controller.reset();
		excecutionMonitor.setRunning(true);
		
	}
	public void setRef(double reference) {
		controllerStatMonitor.setRef(reference);
		ref = reference;
		
	}
	public void shutDown() {
		excecutionMonitor.setRunning(false);
		setRef(0.0);
		
	}
	
	@Override
	public PIDParameters getInnerParameters() {
		return controller.getParameters();
	}
	
	@Override
	public void setInnerParameters(PIDParameters innerPar) {
		controller.setParameters(innerPar);
		
	}
	@Override
	public void setOuterParameters(PIDParameters outerPar) {
		
		
	}
	@Override
	public PIDParameters getOuterParameters() {
		return null;
	}
	
	public double getRef(){
		return ref;
	}
	

	public void setK(double d) {
		innerContParameters.K = d;
		setInnerParameters(innerContParameters);
		
	}

}